#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include <iostream>

#include "geometry_msgs/msg/point.hpp"
#include "ros2_cpp_pkg/action/navigate.hpp"

using geometry_msgs::msg::Point;

typedef ros2_cpp_pkg::action::Navigate NavigateAction;
typedef rclcpp_action::ServerGoalHandle<NavigateAction> GoalHandle;

// Distance in meters which is considered "close enough" to the goal
const float DIS_THRESHOLD = 0.1;

class NavigateActionServerNode: public rclcpp::Node
{
  public:
    NavigateActionServerNode(): Node("navigate_action_server_node")
    {
      robot_postion_ = Point();
      robot_postion_subscrition_ = this->create_subscription<Point>(
        "robot_position", 10,
        std::bind(&NavigateActionServerNode::update_robot_position, this, 
          std::placeholders::_1)
      );
      action_server_ = rclcpp_action::create_server<NavigateAction>(
        this,
        "navigate",
        std::bind(&NavigateActionServerNode::handle_goal, this,
          std::placeholders::_1, std::placeholders::_2),
        std::bind(&NavigateActionServerNode::handle_cancel, this,
          std::placeholders::_1),
        std::bind(&NavigateActionServerNode::handle_accepted, this,
          std::placeholders::_1)
      );
      std::cout << "Navigate Actionn Started" << std::endl;
    }

  private:
    rclcpp_action::GoalResponse handle_goal(
      const rclcpp_action::GoalUUID &uuid,
      std::shared_ptr<const NavigateAction::Goal> goal)
    {
      (void)uuid;

      std::cout << "Recieved goal point: ("
        << goal->goal_point.y << ","
        << goal->goal_point.y << ","
        << goal->goal_point.z << ","
        << std::endl;

      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(
      const std::shared_ptr<GoalHandle> goal_handle)
    {
      (void)goal_handle;
      std::cout << "Recieved request to cancel goal" << std::endl;

      return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(
      const std::shared_ptr<GoalHandle> goal_handle)
    {
      // Start new thread to prevent blocking ROS eyecutor
      std::thread{std::bind(&NavigateActionServerNode::execute, this,
      std::placeholders::_1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<GoalHandle> goal_handle)
    {
      std::cout << "Executing Goal" << std::endl;
      auto start_time = rclcpp::Clock().now();

      const auto goal = goal_handle->get_goal();
      auto feedback = std::make_shared<NavigateAction::Feedback>();
      auto result = std::make_shared<NavigateAction::Result>();
      rclcpp::Rate loop_rate(1); // Rate in Hz to calculate distance, publish feedback

      feedback->distance_to_point = DIS_THRESHOLD; // Init value to trigger while loop

      while (feedback->distance_to_point >= DIS_THRESHOLD)
      {
        feedback->distance_to_point = std::sqrt(
        std::pow(robot_postion_.x - goal->goal_point.x, 2) + 
        std::pow(robot_postion_.y - goal->goal_point.y, 2) + 
        std::pow(robot_postion_.z - goal->goal_point.z, 2));

        goal_handle->publish_feedback(feedback);
        loop_rate.sleep();
      }

      result->elpased_time = (rclcpp::Clock().now() - start_time).seconds();
      goal_handle->succeed(result);
      std::cout << "Goal succeeded" << std::endl;
    }

    void update_robot_position(const Point &msg)
    {
      robot_postion_ = msg;
    }

    rclcpp_action::Server<NavigateAction>::SharedPtr action_server_;
    geometry_msgs::msg::Point robot_postion_;
    rclcpp::Subscription<Point>::SharedPtr robot_postion_subscrition_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<NavigateActionServerNode>());
  rclcpp::shutdown();

  return 0;
}